﻿using System;
using System.Collections.Generic;
using System.Text;
using RobotRace.Geometry;
using FIRADriverLibrary;
using RobotRace.utils.graph;

namespace RobotRace.engine.agents.implementation
{
    public class PIDPathFollower : AbstractPIDFollower
    {

        public double maxDistToUsePath { get; set; }

        public PIDPathFollower(Point2D robotStartingPoint, FIRARobotState controlledRobot, List<Point2D> pathToFollow)
            : base(robotStartingPoint, controlledRobot, pathToFollow)
        {
            maxDistToUsePath = 0.2;
        }

        public override void step()
        {
            if (!robot.owned)
            {
                throw new Exception("ROBOT NOT OWNED!");
            }

            if (path == null || path.Count <= 1)
            {
                return;
            }

            double currentRobotXPosition = robot.x;
            double currentRobotYPosition = robot.y;
            double currentRobotAngle = robot.angle;
            double currentRobotLeftSpeed = robot.leftSpeed;
            double currentRobotRightSpeed = robot.rightSpeed;

            //System.Diagnostics.Debug.WriteLine("<" + currentCheckpoint);
            Node currentNode = checkpointManagementStrategy.findNextPoint(path, new Point2D(robot.x, robot.y));
            //currentCheckpoint = currentNode.nodeNumberOnPath; //findTargetCheckpoint(new Point2D(robot.x, robot.y), pathToFollow, currentCheckpoint);
            // System.Diagnostics.Debug.WriteLine(">" + currentCheckpoint);

            Vector2D distanceVector = new Vector2D(new Point2D(currentRobotXPosition, currentRobotYPosition), currentNode.point);
            Vector2D robotMoveVector = new Vector2D(currentRobotAngle) * (1 + (currentRobotLeftSpeed + currentRobotRightSpeed) / 2);
            double differenceAngle = Vector2D.AngleBetween(robotMoveVector, distanceVector);

            int nextCheckpoint = currentNode.nodeNumberOnPath + 1;
            if (nextCheckpoint >= path.Count)
            {
                nextCheckpoint = 0;
            }

            Node nextNode = path.getNodeWithIdInSequence(nextCheckpoint);

            Vector2D pathDistanceVector = new Vector2D(currentNode.point, nextNode.point);
            double pathDifferenceAngle = Vector2D.AngleBetween(robotMoveVector, pathDistanceVector);

            double distToNextCheckpoint = nextNode.point.GetDistance(currentRobotXPosition, currentRobotYPosition);
            double pathWeight = Math.Max(0.0, maxDistToUsePath - distToNextCheckpoint) / maxDistToUsePath;
            if (Double.IsNaN(pathWeight))
            {
                pathWeight = 0.0;
            }
            double weightedSumOfDifferences = pathWeight * pathDifferenceAngle + (1.0 - pathWeight) * differenceAngle;
            weightedSumOfDifferences = pathDifferenceAngle;

            //System.Diagnostics.Debug.WriteLine(currentCheckpoint + "\t" + robotMoveVector + "\t" + differenceAngle + "\t" + pathDifferenceAngle + "\t" + weightedSumOfDifferences + "\t" + distToNextCheckpoint + "\t" + pathWeight);

            //PID start
            robot.leftSpeed = startingStepSpeed;
            robot.rightSpeed = startingStepSpeed;

            sumOfErrors = (2.0 / 3.0) * sumOfErrors + weightedSumOfDifferences;
            double turn = weightedSumOfDifferences * turningSpeedModifier + sumOfErrors * addedErrorWeight + (weightedSumOfDifferences - previousError) * addedDerivativeWeight;
            robot.leftSpeed = robot.leftSpeed + turn;
            robot.rightSpeed = robot.rightSpeed - turn;

            previousError = weightedSumOfDifferences;
            //PID end

            //make sure we are not going to fast
            obeyMaxVelocityLimit();
        }
    }
}
